import threading
import rclpy
from queue import Queue
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Twist, PoseStamped
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from tf_transformations import quaternion_from_euler


class ROS2Client:
    """ROS2客户端，用于直接与ROS2系统通信"""

    def __init__(self):
        # 地图数据存储
        self.map_data = None
        self.last_map_hash = None

        # 速度指令管理
        self.last_vel_cmd = None
        self.vel_cmd_queue = Queue()  # 线程安全的指令队列
        self.is_running = True

        # 初始化ROS2节点
        rclpy.init()
        self.node = rclpy.create_node('web_control_client')

        # 地图订阅者
        self.map_subscriber = self.node.create_subscription(
            OccupancyGrid,
            '/map',
            self._on_map_received,
            10  # QoS配置
        )

        # 速度指令发布者
        self.vel_publisher = self.node.create_publisher(
            Twist,
            '/cmd_vel',
            10
        )

        # 导航Action客户端
        self.nav_client = ActionClient(self.node, NavigateToPose, 'navigate_to_pose')

        # 启动ROS2节点自旋线程
        self.ros_spin_thread = threading.Thread(target=self._spin_ros_node, daemon=True)
        self.ros_spin_thread.start()

        # 启动速度指令发布线程
        self.cmd_pub_thread = threading.Thread(target=self._publish_vel_commands, daemon=True)
        self.cmd_pub_thread.start()

        print("[ROS2Client] 初始化完成（无rosbridge依赖）")

    def _spin_ros_node(self):
        """ROS2节点自旋循环"""
        while rclpy.ok() and self.is_running:
            rclpy.spin_once(self.node, timeout_sec=0.1)

    def _publish_vel_commands(self):
        """速度指令发布线程（20Hz频率）"""
        while self.is_running:
            if not self.vel_cmd_queue.empty():
                linear_x, angular_z = self.vel_cmd_queue.get()
                twist = Twist()
                twist.linear.x = linear_x
                twist.angular.z = angular_z
                self.vel_publisher.publish(twist)
                self.vel_cmd_queue.task_done()
            # 控制发布频率
            threading.Event().wait(0.05)

    def _on_map_received(self, msg: OccupancyGrid):
        """处理收到的地图消息"""
        # 通过时间戳哈希判断地图是否更新
        current_hash = hash((msg.info.map_load_time.sec, msg.info.map_load_time.nanosec))
        if current_hash != self.last_map_hash:
            self.last_map_hash = current_hash
            self.map_data = msg
            print(f"[地图更新] 尺寸: {msg.info.width}x{msg.info.height}px")

    def send_velocity_command(self, linear_x: float, angular_z: float):
        """发送速度指令到队列（带范围限制）"""
        # 限制速度范围
        linear_x = max(-1.0, min(1.0, linear_x))
        angular_z = max(-2.0, min(2.0, angular_z))

        # 避免重复发送相同指令
        if self.last_vel_cmd == (linear_x, angular_z):
            return
        self.last_vel_cmd = (linear_x, angular_z)

        self.vel_cmd_queue.put((linear_x, angular_z))
        print(f"[指令入队] linear_x={linear_x}, angular_z={angular_z}")

    def send_navigation_goal(self, x: float, y: float, theta: float = 0.0):
        """发送导航目标点"""
        print(f"[导航目标] x={x}, y={y}, θ={theta}")

        # 计算旋转四元数
        q = quaternion_from_euler(0, 0, theta)

        # 构建位姿消息
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = self.node.get_clock().now().to_msg()
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.orientation.x = q[0]
        pose.pose.orientation.y = q[1]
        pose.pose.orientation.z = q[2]
        pose.pose.orientation.w = q[3]

        # 构建导航目标消息
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = pose

        # 发送导航目标
        self.nav_client.wait_for_server()
        future = self.nav_client.send_goal_async(goal_msg)
        future.add_done_callback(self._on_goal_response)

    def _on_goal_response(self, future):
        """处理导航目标响应"""
        goal_handle = future.result()
        if not goal_handle.accepted:
            print("[导航状态] 目标被拒绝")
            return
        print("[导航状态] 目标已接受")
        future = goal_handle.get_result_async()
        future.add_done_callback(self._on_goal_complete)

    def _on_goal_complete(self, future):
        """导航完成回调"""
        print("[导航状态] 导航完成")

    def shutdown(self):
        """停止ROS2客户端"""
        self.is_running = False
        rclpy.shutdown()
        print("[ROS2Client] 已停止")